package simulator;
import java.util.ArrayList;
import java.awt.*;
/**
 * RangeFinder models the behavior of a generic distance sensor (Ultrasonic, IR)
 * providing distance values from object(s) in front of it
 * @author anand
 *
 */
public class RangeFinder extends Sensor{
	public static final int HISTORY_SIZE = 7;


	private double range;
	private double[] history;
	private int pointer;
	private double mountangle; // angle at which the RangeFinder with respect to the front of the bot
	//private static final double TOLERANCE = 180.0;  //degrees of tolerance in contact
	private Color color = new Color((float) Math.random(),(float) Math.random(),(float) Math.random());
	public RangeFinder(Environment e, double x, double y, double range){
		super(e);
		this.x=x;
		this.y=y;
		this.range=range;
		mountangle=0;
		history = new double[HISTORY_SIZE];
		pointer = 0;
	}

	public RangeFinder(Environment e, double x, double y, double range, double angle){
		super(e);
		this.x=x;
		this.y=y;
		this.range=range;
		mountangle=Math.toRadians(angle);
		history = new double[HISTORY_SIZE];
		pointer = 0;
	}

	public int getID() {
		return 1;
	}

	public static double deviation(double[] history){
		double mean = average(history);
		int max = history.length-1;
		int min = 0;
		for(int i=0; i<history.length; i++){
			if(history[i]<=history[min]){
				min = i;
			}else if(history[i]>=history[max]){
				max = i;
			}
		}
		double sum =0;   
		for(int j=0; j<history.length; j++){
			if(j!= max && j!=min){
				sum +=((mean - history[j])*(mean - history[j]));
			}
		}
		sum = Math.sqrt(sum/(history.length-2));
		return sum;
	}

	public static double average(double[] history){
		int max = history.length-1;
		int min = 0;
		for(int i=0; i<history.length; i++){
			if(history[i]<=history[min]){
				min = i;
			}else if(history[i]>=history[max]){
				max = i;
			}
		}
		double sum = 0;
		for(int j=0; j<history.length; j++){
			if(j!= max && j!=min){
				sum += history[j];
			}
		}
		sum/=history.length-2;
		return sum;
	}

	public double[] read(int magicnum) {  
		double[] reading = new double[3];
		reading[0]=range;             
		ArrayList<PhysicalObject> known=env.getLocalObjects((env.getRobotX() + x) ,														(env.getRobotY() + y) , range);

		for(PhysicalObject o : known) {
			for(int i=0; i<range; i++) {
				if(o.intersects(env.getRobotX() + ((x + i) *
						(Math.cos(env.getRobotAngle()+mountangle))),
						env.getRobotY() + ((y + i) *
								(Math.sin(env.getRobotAngle() +mountangle))))){
					reading[0]=Math.min(i,reading[0]);
					break;
				}
			}

		}
		history[pointer] = reading[0];
		pointer++;
		pointer%=HISTORY_SIZE;
		reading[1] = deviation(history);
		reading[2] = (double) Simulator.time;
		double fil = average(history);
		//System.out.println(reading[0]);
		setState(reading);
		return reading;
	}

	public String getName(){
		return "RangeFinder";  //add id as well?
	}

	public double getRange(){
		return range;
	}

	public void paint(Graphics g) {
		try {
			g.setColor(Color.BLACK);

			Graphics2D g2d = (Graphics2D)g;			int width = 3;			g2d.setStroke(new BasicStroke(width));			g.drawLine(
					(int)(env.getRobotX() + ((x + 5) * Math.cos(env.getRobotAngle() + mountangle))),
					(int)(env.getRobotY() + ((y + 5) * Math.sin(env.getRobotAngle() + mountangle))),
					(int)(env.getRobotX() + ((x + Math.min(10, getState()[0])) * Math.cos(env.getRobotAngle() + mountangle))),
					(int)(env.getRobotY() + ((y + Math.min(10, getState()[0])) * Math.sin(env.getRobotAngle() + mountangle)))
			);

			width = 1;			g2d.setStroke(new BasicStroke(width));			g.setColor(this.color);
			g.drawLine(
					(int)(env.getRobotX() + ((x + Math.min(10, getState()[0])) * Math.cos(env.getRobotAngle() + mountangle))),
					(int)(env.getRobotY() + ((y + Math.min(10, getState()[0])) * Math.sin(env.getRobotAngle() + mountangle))),
					(int)(env.getRobotX() + ((x + getState()[0]) * Math.cos(env.getRobotAngle() + mountangle))),
					(int)(env.getRobotY() + ((y + getState()[0]) * Math.sin(env.getRobotAngle() + mountangle)))
			);
		}catch(Exception e) {e.printStackTrace(Simulator.out);}
	}

	public double getMountAngle() { return mountangle; }
	public void setMountAngle(double newangle){
		mountangle=newangle;
	}


}